#include <iostream>
#include <cstring>
#include <sys/mman.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <unistd.h>
#include <semaphore.h>
#include <chrono>
#include <thread>
#include <iomanip> 
#include <sstream> 
#include <sys/mman.h>
#include <sys/stat.h>
#include <semaphore.h>
#include <sys/types.h>
#include <cstdlib>
#include <ctime>
#include <mutex>

#define DATA_SIZE 64

typedef struct
{
    uint8_t Stand;
    uint8_t forward;
    uint8_t back;
    uint8_t Left;
    uint8_t Right;
}Trace_Control_typedef;

typedef struct
{
    uint8_t SoftWareVer;
    uint8_t SoftWareBuildVer;
    uint8_t HardWareVer;
    uint8_t BootLoaderVer;
}Trace_Ver_typedef;

typedef struct 
{
	int32_t U_kk;    	    // The amount of the last output
	int32_t ekk;		 	// The last input deviation
	int32_t ekkk;			// The input deviation of the previous time
	int32_t Ur;				// The limiting output value needs to be initialized
	int32_t Kp;				// scale
	int32_t Ki;				// integration
	int32_t Kd;				// differential
	
	uint8_t En;             // switch
	int16_t Adjust;         // Adjust the amount
	int16_t ValSet;       	// Speed settings
	int16_t ValNow;       	// Current speed

}Trace_PID_typedef;

typedef struct
{
    Trace_Ver_typedef       Ver;
    Trace_PID_typedef       Pid_Left;
    Trace_PID_typedef       Pid_Right;
    Trace_PID_typedef       Pid_Stand;
    Trace_PID_typedef       Pid_Ster;
    Trace_Control_typedef   Move;

}Trace_Data_Typedef;


#define SHM_NAME_RECV "auto_netshm_recv"
#define SHM_NAME_SEND "auto_netshm_send"

enum DataState {
    DataState_Idle,
    DataState_Working,  
};

typedef enum
{
	E_CMD_HANDSHAKE      = 0x01, 
    E_CMD_READVER        = 0x02,
    E_CMD_HEARTBEAT      = 0x03,
    
    E_CMD_DIAG           = 0x0D,
    E_CMD_ERROR          = 0x0E,
    E_CMD_IFC            = 0x0F,

    E_CMD_PID_LEFT       = 0x10,
    E_CMD_PID_RIGHT,
    E_CMD_PID_STAND,
    E_CMD_PID_STER,
    E_CMD_MOVE,

    E_CMD_MAX,

}TRACE_CMD_ENUM;

typedef struct{
    sem_t Sem_Notice;          // tell other program that data is ready
    uint8_t DataState;         // data state
    TRACE_CMD_ENUM DataType;   // data type
    Trace_Data_Typedef Data;  // data
}SharedMemory_TypeDef;

void receiveDataFromShm() {
    // 打开接收共享内存
    int shm_recv_fd = shm_open(SHM_NAME_RECV, O_RDWR, 0666);
    if (shm_recv_fd == -1) {
        perror("Error opening SHM_NAME_RECV");
        return;
    }

    SharedMemory_TypeDef *sharedMemoryRecv = (SharedMemory_TypeDef *)mmap(nullptr, sizeof(SharedMemory_TypeDef), PROT_READ | PROT_WRITE, MAP_SHARED, shm_recv_fd, 0);
    if (sharedMemoryRecv == MAP_FAILED) {
        perror("Error mapping SHM_NAME_RECV");
        close(shm_recv_fd);
        return;
    }
    std::cout << "Shared memory address in app2: " << sharedMemoryRecv << std::endl;
    //std::cout << "Shared memory data: " << sharedMemoryRecv->Data << std::endl;
    while (true) {
        int semValue;
        sem_getvalue(&sharedMemoryRecv->Sem_Notice, &semValue);
        std::cout << "Semaphore value before wait: " << semValue << std::endl;

        sem_wait(&sharedMemoryRecv->Sem_Notice);

        if(sharedMemoryRecv->DataState == DataState_Working)
        {
            auto now = std::chrono::system_clock::now();
            std::time_t now_time = std::chrono::system_clock::to_time_t(now);
            auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
            std::ostringstream timestampStream;
            timestampStream << std::put_time(std::localtime(&now_time), "%Y-%m-%d %H:%M:%S") << "." << std::setfill('0') << std::setw(3) << milliseconds.count();
            std::string timestamp = timestampStream.str();
            std::string receivedData = "[" + timestamp + "] " +
                               std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.SoftWareVer)) + "." +
                               std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.SoftWareBuildVer)) + " " +
                               std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.HardWareVer)) + " " +
                               std::to_string(static_cast<int>(sharedMemoryRecv->Data.Ver.BootLoaderVer));

            std::cout << "Received: "<< receivedData << std::endl;

            sharedMemoryRecv->DataState = DataState_Idle;  
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    munmap(sharedMemoryRecv, sizeof(SharedMemory_TypeDef));
    close(shm_recv_fd);
}

void sendDataToShm() {

    int shm_send_fd = shm_open(SHM_NAME_SEND, O_RDWR, 0666);
    if (shm_send_fd == -1) {
        perror("Error opening SHM_NAME_SEND");
        std::cerr << "Error: Unable to open shared memory for sending." << std::endl;
        return;
    }

    SharedMemory_TypeDef *sharedMemorySend = (SharedMemory_TypeDef *)mmap(
        nullptr, sizeof(SharedMemory_TypeDef), PROT_READ | PROT_WRITE, MAP_SHARED, shm_send_fd, 0);
    if (sharedMemorySend == MAP_FAILED) {
        std::cerr << "Error: Unable to map shared memory for sending." << std::endl;
        close(shm_send_fd);
        return;
    }

    int counter = 0;
    while (true) {

        if (sharedMemorySend->DataState == DataState_Idle) {
            std::string message = "Test message " + std::to_string(counter++);
            //strncpy(&sharedMemorySend->Data, message.c_str(), DATA_SIZE);
            sharedMemorySend->DataState = DataState_Working;

            sem_post(&sharedMemorySend->Sem_Notice);
            std::cout << "Sent to SHM_NAME_SEND: " << message << std::endl;
        }
        std::this_thread::sleep_for(std::chrono::seconds(1));
    }

    munmap(sharedMemorySend, sizeof(SharedMemory_TypeDef));
    close(shm_send_fd);
}

int main() {
    std::thread recvThread(receiveDataFromShm);
    std::thread sendThread(sendDataToShm);

    recvThread.join();
    sendThread.join();

    return 0;
}
